#include "logic.hpp"
#include "config.hpp"

using namespace asio;
using namespace cv;
#define ISCORRECT(i,w,h) (abs(GET_CENTER(i).x-w/2)<40&&abs(GET_CENTER(i).y-h/2)<30)

Vision* major_Vision;
Vision* sub_Vision;
base_controller* major_Controller;

float xp,yp,xi,yi,xd,yd,xr,yr,wr,hr,xq,yq,wq,hq;
float lmp,lmi,lmd;
int yawOver = 1;
int xslow,//speed when aiming
	xmedium,//speed when colliding ball
	xfast;//speed when crossing
float yawthrott;//threshold when finding box
int choosePath = 0;//0 means choose right landmark if there were two
int qualifiedBall = 100,maxBall = 300;
int qualifiedFrame = 80;
PID_incre pidx(xp,xi,xd,300,-300);
PID_incre pidy(yp,yi,yd,250,-250);
PID_incre pidlm(lmp,lmi,lmd,300,-300);
SimpleKF KFx(320,xq,xr);
SimpleKF KFy(240,yq,yr);
SimpleKF KFw(80,wq,wr);
SimpleKF KFh(80,hq,hr);
//logical func
//first: aim to the box by r
//then: approach the box,get its orientation meanwhile
//then: adjust direction while keeping the box aimed
//then: go through when correct;
int TARGETSIZE = 200;
int MINSIZETOCONTROL = 100;
int MAXSIZETOCONTROL = 400;
std::function<void()> func3 = [](){
	Stats pattern = MIDDLE_STATE;//行为状态;
	Veloc temp_vel={0};
	float yawRec = major_Controller->pose.yawmsg;
	const int yawOverTemp = yawOver;
	bool finishFinding = 0;
	while(1)
	{
		if(major_Controller->readyToUse()&&!major_Vision->undistort.empty())
		{
			if(major_Controller->droneStatus.sys_status!=MAV_STATE_STANDBY&&major_Controller->droneStatus.sys_status!=MAV_STATE_ACTIVE)
				major_Controller->arm_disarm(1);
			if(pattern!=TRACKING_LANDMARK)
				major_Vision->detectClassifier(1);//detect and draw all boxes on "Mat undistort"
			imshow("a",major_Vision->undistort);
			waitKey(1);
			bbox_t posTarget = major_Vision->targetFrame;
			posTarget.x =KFx.update(major_Vision->targetFrame.x);
			posTarget.y =KFy.update(major_Vision->targetFrame.y);
			posTarget.w =KFw.update(major_Vision->targetFrame.w);
			posTarget.h =KFh.update(major_Vision->targetFrame.h);
			temp_vel={0};
/**************************************************************************************************************/
/**************************************************************************************************************/
/**************************************************************************************************************/
			if(major_Vision->newPic&&sub_Vision->newPic&&pattern==MIDDLE_STATE)
			{
				sub_Vision->detectClassifier(1);
				if(sub_Vision->targetLandmarks.size()==0&&
				BOXSIZE(major_Vision->targetFrame)<qualifiedFrame&&
				BOXSIZE(major_Vision->targetBall)<qualifiedBall){
					temp_vel.r = 100*yawOver;
					major_Controller->setTargetVel(temp_vel);
					if(abs(major_Controller->pose.yawmsg-yawRec)>yawthrott)
						yawOver = -1;
					continue;
				}//if no landmark detected, swing around to find one
				yawRec = major_Controller->pose.yawmsg;
				yawOver = yawOverTemp;
				if(BOXSIZE(sub_Vision->targetLandmark)>qualifiedLandmark)
					pattern = TRACKING_LANDMARK;
				if(BOXSIZE(major_Vision->targetFrame)>qualifiedFrame)
					pattern = CORRECTING_FRAME_POSITION;
				if(BOXSIZE(major_Vision->targetBall)>qualifiedBall)
					pattern = COLLIDING_BALL;
			}

			else if(sub_Vision->newPic&&pattern==TRACKING_LANDMARK)
			{
				sub_Vision->detectClassifier(1);
				bbox_t target = sub_Vision->targetLandmark;
				if(sub_Vision->targetLandmarks.size()==0){
					pattern = MIDDLE_STATE;
					continue;
				}
				if(sub_Vision->targetLandmarks.size()>1)
				{
					bbox_t temp_L={0},temp_R={0};
					temp_L.x = 320;temp_R.x=320;
					for(auto i:sub_Vision->targetLandmarks){
						if(i.x>temp_R.x)temp_R=i;
						if(i.x<temp_L.x)temp_L=i;
					}
					if(choosePath==1)target = temp_L;
					else target = temp_R;
				}
				temp_vel.y = pidlm.update(sub_Vision->WIDTH/2,GET_CENTER(sub_Vision->targetLandmark).x);
				temp_vel.x = xslow+50;

				if(target.obj_id==L_LANDMARKID)
					temp_vel.r = -100;
				if(target.obj_id==R_LANDMARKID)
					temp_vel.r = 100;
				if(target.obj_id==E_LANDMARKID&&
				abs(GET_CENTER(target).x-sub_Vision->WIDTH/2)<40)
					pattern = MIDDLE_STATE;
			}

			else if(major_Vision->newPic&&pattern==CORRECTING_FRAME_POSITION)
			{
				if(BOXSIZE(posTarget)>MAXSIZETOCONTROL){
					temp_vel.x = -150;
					major_Controller->setTargetVel(temp_vel);
					continue;
				}
				if(BOXSIZE(posTarget)<MINSIZETOCONTROL){
					temp_vel.x = 150;
					major_Controller->setTargetVel(temp_vel);
					continue;
				}
				if(BOXSIZE(posTarget)>TARGETSIZE&&
				ISCORRECT(posTarget,major_Vision->WIDTH,major_Vision->HEIGHT))
					pattern = CORRECTING_FRAME_ORIEN;
				
				temp_vel.r = pidx.update(major_Vision->WIDTH/2,GET_CENTER(posTarget).x);
				temp_vel.z = pidy.update(major_Vision->HEIGHT/2,GET_CENTER(posTarget).y);
				temp_vel.x = xslow;
				temp_vel.y = 0;

			}		
			else if(major_Vision->newPic&&pattern==CORRECTING_FRAME_ORIEN)
			{
				temp_vel.r = pidx.update(major_Vision->WIDTH/2,GET_CENTER(posTarget).x);
				temp_vel.z = pidy.update(major_Vision->HEIGHT/2,GET_CENTER(posTarget).y);
				temp_vel.x = xslow;
				int currentBoxOrientation=0;
				currentBoxOrientation+=major_Vision->targetFrameOrint;
				if(currentBoxOrientation>0)
					temp_vel.y = -150;
				else
					temp_vel.y = 150;

				//如果框大小不合适，退回POSITION
				if(BOXSIZE(posTarget)+10<TARGETSIZE||
				BOXSIZE(posTarget)>MAXSIZETOCONTROL)
					pattern = CORRECTING_FRAME_POSITION;
				if(ISCORRECT(posTarget,major_Vision->WIDTH,major_Vision->HEIGHT)&&
				posTarget.obj_id==EXACTBOXID)
				{//如果方向正确，框居中，向前
					pattern = GOING_DIRECT_THROUGH;
					continue;
				}
			}
			else if(major_Vision->newPic&&pattern==GOING_DIRECT_THROUGH)
			{
				temp_vel.x = xfast;

				if(!(ISCORRECT(posTarget,major_Vision->WIDTH,major_Vision->HEIGHT)&&
				1.0*posTarget.w/(1.0*posTarget.h)>0.8)&&BOXSIZE(posTarget)<MAXSIZETOCONTROL){
					//如果方向有误且还有控制空间
					pattern = CORRECTING_FRAME_POSITION;
					continue;
				}
				if(BOXSIZE(posTarget)<MINSIZETOCONTROL){
					pattern = MIDDLE_STATE;
				}
			}
			else if(major_Vision->newPic&&pattern==COLLIDING_BALL)
			{
		
				temp_vel.r = pidx.update(major_Vision->WIDTH/2,GET_CENTER(major_Vision->targetBall).x);
				temp_vel.z = pidy.update(major_Vision->HEIGHT/2,GET_CENTER(major_Vision->targetBall).y);
				temp_vel.x = xslow;
				if(BOXSIZE(major_Vision->targetBall)<qualifiedBall&&BOXSIZE(major_Vision->targetBall)>0||
				(BOXSIZE(major_Vision->targetBall)>maxBall||BOXSIZE(major_Vision->targetBall)==0))
				//finished or cannot collide
					pattern = MIDDLE_STATE;
				if(ISCORRECT(major_Vision->targetBall,major_Vision->WIDTH,major_Vision->HEIGHT))
					temp_vel.x = xmedium;
			}
/**************************************************************************************************************/
/**************************************************************************************************************/
/**************************************************************************************************************/
		}
		else
		{
			temp_vel = {0};
			Sleep(500);
			cout<<"abnormal"<<endl;
		}
		major_Controller->setTargetVel(temp_vel);
	}
};


